#include <ros/ros.h>
#include "trigger_response/event.h"
#include "trigger_response/EventClient.h"
#include <typeinfo>

// void ClientPoint(trigger_response::event::Request &req,trigger_response::event::Request &res)
// {
//     // float64[] UavPos # 无人机的三维位置,xyz
//     // float64[] UgvPos # 无人车的三维位置,xyz
//     // float64[] TargetPos # 无人车目标点的三维位置,xyz,都为-1表示无人车处于静止状态
//     // # float64 Tardis # 无人车与目标点的距离 
//     // int32[] EType # 随遇事件类型,依次为 (其他,人,车,火力点),0表示没有,1表示有 0001~1111 共15种
//     // float64[] eventLocx # 每个突发事件的三维位置,顺序与上面一致,没有,使用-1替代
//     // float64[] eventLocy 
//     // float64[] eventLocz 

// }

require::require(const ros::NodeHandle &nh, const string& nodeName)
    :nh_(nh),Nodename_(nodeName){

       // ros::ServiceServer service = nh.advertiseService("enventHandle",ClientPoint);
        // 创建服务客户端


    air_odom_sub = nh_.subscribe<nav_msgs::Odometry>("/air_bodyinworld/odom", 1, &require::odomCallback, this);
    // std::cout << "xxxxxxxx" << std::endl;
    car_odom_sub = nh_.subscribe<nav_msgs::Odometry>("/car_bodyinworld/odom", 1, &require::odomCallback, this);
    robot_odom_sub = nh_.subscribe<nav_msgs::Odometry>("robot_1", 1, &require::odomCallback, this);

    client = nh_.serviceClient<trigger_response::event>("eventHandle");
    // ros::service::waitForService("eventHandle");  // 等待服务可用

    // list_service_sub = nh_.subscribe<std::int32_t>("list_trigger",1, std::bind(&handle::ListServiceCallback, &nh_, std::placeholders::_1));
    list_service_sub = nh_.subscribe<std_msgs::Int32>("list_trigger", 1, &require::ListServiceCallback, this);

    
}

void require::ListServiceCallback(const std_msgs::Int32::ConstPtr& msg){
    // 锁定对应的互斥锁以保护共享数据
    // std::lock_guard<std::mutex> lock_air(air_pose_mutex);
    // std::lock_guard<std::mutex> lock_car(car_pose_mutex);
    // std::lock_guard<std::mutex> lock_robot(robot_pose_mutex);
    // air_pose.clear();
    // car_pose.clear();
    // robot_pose.clear();

    // air_odom_sub = nh_.subscribe<nav_msgs::Odometry>("/air_bodyinworld/odom", 10, &handle::odomCallback, this);
    // std::cout << "xxxxxxxx" << std::endl;
    // car_odom_sub = nh_.subscribe<nav_msgs::Odometry>("/car_bodyinworld/odom", 10, &handle::odomCallback, this);
    // robot_odom_sub = nh_.subscribe<nav_msgs::Odometry>("robot_1", 10, &handle::odomCallback, this);
    // std::cout<< msg->data<<std::endl;
    // if 
    if (robot_pose.empty()){
        return;
    }

    if (robot_pose.size() > 0){
        robotEType = 1;
        robot_x =robot_pose[0];
        robot_y =robot_pose[1];
        robot_z =robot_pose[2];
    }else{
        robotEType = 0;
        robot_x = -1;
        robot_y = -1;
        robot_z = -1;
    }
    // std::cout << "ListServiceCallback" << std::endl;


    // 构建服务请求
    trigger_response::event srv;
    // std::cout << "air_pose[0]" << air_pose[0] << std::endl;
    srv.request.UavPos = {air_pose[0], air_pose[1], air_pose[2]};
    // std::cout << "11111111111111" << std::endl;
    srv.request.UgvPos = {car_pose[0], car_pose[1], car_pose[2]};
    // std::cout << "444444444444" << std::endl;
    srv.request.TargetPos = {robot_pose[0], robot_pose[1], robot_pose[2]};
    // std::cout << "3333333333333" << std::endl;
    srv.request.EType = {0, robotEType, 0, 0};
    srv.request.eventLocx = {-1, robot_x, -1, -1};
    srv.request.eventLocy = {-1, robot_x, -1, -1};
    srv.request.eventLocz = {-1, robot_x, -1, -1};

    // // 发送请求并接收响应
    bool flag = client.call(srv);
    ROS_INFO("Sending a request, response: %s", flag ? "Success" : "Failed");

    // return true;

}

void require::odomCallback(const nav_msgs::Odometry::ConstPtr& msg) {
    // std::cout << "333333333333333" << std::endl;
    // 检查child_frame_id以确定是哪个话题的回调
    if (msg->child_frame_id == "PX4/base_link") {
        // std::lock_guard<std::mutex> lock(air_pose_mutex);
        air_pose = {static_cast<float>(msg->pose.pose.position.x), static_cast<float>(msg->pose.pose.position.y), static_cast<float>(msg->pose.pose.position.z)};
        // ROS_INFO("This is the PX4/base_link odometry callback.");
        // std::cout << "air_pose[0]" << air_pose[2] << std::endl;
    } else if (msg->child_frame_id == "Ground0/base_link") {
        // std::lock_guard<std::mutex> lock_car(car_pose_mutex);
        car_pose = {static_cast<float>(msg->pose.pose.position.x), static_cast<float>(msg->pose.pose.position.y), static_cast<float>(msg->pose.pose.position.z)};
        // ROS_INFO("This is the odometry callback for another frame: %s", msg->child_frame_id.c_str());
    } else if (msg->child_frame_id == "robot"){
        // std::lock_guard<std::mutex> lock_robot(robot_pose_mutex);
        robot_pose = {static_cast<float>(msg->pose.pose.position.x), static_cast<float>(msg->pose.pose.position.y), static_cast<float>(msg->pose.pose.position.z)};
        // ROS_INFO("This is the odometry callback for another frame: %s", msg->child_frame_id.c_str());
    }

    // return true;
}





int main(int argc,char **argv)
{
    string nodeName = "eventClient";
    ros::init(argc,argv,nodeName);
    ros::NodeHandle nh;


    require eh(nh,nodeName);
    ROS_INFO("event created");


    ros::spin();
    return 0;

    // trigger_response::event ai;
    // ai.request.UavPos={1,1,6};
    // ai.request.UgvPos={1,1,1};
    // ai.request.TargetPos={1,4,1};
    // ai.request.EType ={0,0,1,1};
    // ai.request.eventLocx={-1,-1,2,1};
    // ai.request.eventLocy={-1,-1,2,2};
    // ai.request.eventLocz={-1,-1,1,1};
    // bool flag = client.call(ai);
    // ROS_INFO("Sending a request");
    
    // if(flag){
    //     ROS_INFO("result : %d",ai.response.response);
    // }else{
    //     ROS_ERROR("handling failed");
    // }
}